//
// Created by az on 2020/7/15.
//

#include "ATTOM_nav.h"

nav_msgs::OccupancyGrid ESDF_map_;
wmr_msgs::PolynomialTrajectory traj_;
std::vector<std::vector<double>> dist_grid_;
//speed command
ros::Publisher cmd_pub_, traj_Stime_pub_, restart_pub_;
geometry_msgs::Twist speed_command_;
//navigation control
ATTOM *attom_;
//robot state
bool has_pos_ = false, has_map_ = false, has_traj_ = false;
bool is_auto_ = false, is_angle_PID_ = false;
Eigen::Vector3d pt_robot_, vel_3d_, eular_, avl_3d_;
Eigen::Quaterniond quan_robot_;
double robot_vel_, robot_avl_, robot_yaw_;
double minimize_order_, traj_order_;
Eigen::VectorXd C_, Cv_, Ca_, Cj_;
Eigen::VectorXd getTrajPoint(double time);

std::ofstream out_file_;
std::mutex file_mutex_, map_lock_;

std::string double2string(const double value) {
    std::ostringstream out;
    out.precision(15);
    out << value;
    return out.str();
}
/**  提示规划模块--该重新规划了 ***/
/**  传入参数--无可通行路径、离参考轨迹太远(true) ***/
/**  此时publish(false) 请求规划器清楚所有目标点 ***/
inline void restartTraj(bool mark){
    std_msgs::Bool restart_traj;
    restart_traj.data = !mark;
    restart_pub_.publish(restart_traj);
}

/**  目标点信息回调函数 --接收目标点  --wp: xyz(m)  **/
void rcvWaypointCallback(const nav_msgs::Path &wp){
    std::string waypoint_type = wp.header.frame_id;
    if(waypoint_type == "HANDLE"){
        attom_->resetMark();
        if(is_angle_PID_)
            attom_->setPIDMark(true);
    }
}

/**  位置信息回调函数 -- state: -xyz(m), -quanter, -v(m/s) -w(rad/s)  **/
double t_traj_start_ = 0, t_traj_sum_ = 0;
ros::Time t_now_;
oriTrans orientationTrans_;
void rcvPositionCallback(const nav_msgs::Odometry &state){
    if (state.header.frame_id != "wmr")
        return ;

    if(std::isnan(state.pose.pose.position.x) ||
       std::isnan(state.pose.pose.position.y) ||
       std::isnan(state.pose.pose.position.z)  )
        return;

    t_now_ = state.header.stamp;

    pt_robot_(0) = state.pose.pose.position.x;
    pt_robot_(1) = state.pose.pose.position.y;
//    start_pt_(2) = state.pose.pose.position.z;
    pt_robot_(2) = 0.01;

    vel_3d_(0) = state.twist.twist.linear.x;
    vel_3d_(1) = state.twist.twist.linear.y;
    vel_3d_(2) = state.twist.twist.linear.z;

    quan_robot_.w() = state.pose.pose.orientation.w;
    quan_robot_.x() = state.pose.pose.orientation.x;
    quan_robot_.y() = state.pose.pose.orientation.y;
    quan_robot_.z() = state.pose.pose.orientation.z;
    Eigen::Matrix3d DCM;
    Eigen::Vector3d Eular;
    orientationTrans_.Quater2DCM(quan_robot_, &DCM);
    orientationTrans_.DCM2Euler(DCM, &Eular);
    //当前机器人的信息
    robot_yaw_ = Eular(2);

    has_pos_   = true;

    robot_avl_ = state.twist.twist.angular.z;
    robot_vel_ = sqrt(pow(vel_3d_(0), 2) + pow(vel_3d_(1), 2));

    attom_->setRobotState(pt_robot_, robot_yaw_, robot_vel_, robot_avl_);
    //手动模式， 不发送速度指令
    if(!is_auto_) return;
    //判断是否进行跟踪控制
    if (has_pos_ && has_map_ && has_traj_) {
        attom_->run(speed_command_.linear.x, speed_command_.angular.z, is_angle_PID_);
        cmd_pub_.publish(speed_command_);

        //到达目标点或无同执行速度指令时， 放弃执行
        if (attom_->getReachMark() || attom_->getReTrajMark()){
            //机器人不进行跟踪控制的情况：
            //1，无轨迹、位置、地图
            //2，到达目标点,无可行走空间,控制期间离参考轨迹太远
            restartTraj(attom_->getReTrajMark());

            has_traj_ = false;
            attom_->resetMark();
        }
    } else {
        speed_command_.linear.x  = 0;
        speed_command_.angular.z = 0;
        cmd_pub_.publish(speed_command_);
    }
}

void rcvMapCallbak(nav_msgs::OccupancyGrid msg){
    if(msg.data.empty())
        return;
    ESDF_map_.data.clear();

    ESDF_map_.header.frame_id = msg.header.frame_id;
    ESDF_map_.header.stamp    = msg.header.stamp;

    ESDF_map_.info.resolution = msg.info.resolution;
    ESDF_map_.info.width      = msg.info.width;
    ESDF_map_.info.height     = msg.info.height;
    ESDF_map_.info.origin     = msg.info.origin;
    ESDF_map_.data            = msg.data;

    map_lock_.lock();
    {
        std::vector<std::vector<double>>().swap(dist_grid_);
    }
    dist_grid_.resize(ESDF_map_.info.width);
    for (int i = 0; i < (int)dist_grid_.size(); ++i)
        dist_grid_[i].resize(ESDF_map_.info.height);

    for (int x = 0; x < (int)ESDF_map_.info.width; ++x)
        for (int y = 0; y < (int)ESDF_map_.info.height; ++y) {
            dist_grid_[x][y] = (double)ESDF_map_.data[x*ESDF_map_.info.height + y] * ESDF_map_.info.resolution;
        }
    map_lock_.unlock();

    has_map_ = true;

}

double getDistanceCost(const double& _x, const double& _y){
    int ix, iy;
    auto& map_origin   = ESDF_map_.info.origin;
    auto& resolution = ESDF_map_.info.resolution;
    ix = (int)((_x - map_origin.position.x) / resolution);
    iy = (int)((_y - map_origin.position.y) / resolution);

    if (ix > (int)ESDF_map_.info.width || iy > (int)ESDF_map_.info.height || ix < 0 || iy < 0){
        return 0;
    }

    map_lock_.lock();
    double& value = dist_grid_[ix][iy];
    //假设点(_x, _y)在机器人附近， 重置该值为可通行值
    if(value <= 0){
        if(pow((_x - pt_robot_[0]), 2) + pow((_y - pt_robot_[1]), 2) <= pow(0.2, 2))
            value = resolution;
    }
    map_lock_.unlock();

    return value;
}

void rcvIsAutoMode(std_msgs::Bool msg){
    is_auto_ = msg.data;
    if(!is_auto_){
        has_traj_ = false;
    }
}

Eigen::MatrixXd polyCoeff;
void rcvTrajCallback(wmr_msgs::PolynomialTrajectory msg){

    if(msg.action == wmr_msgs::PolynomialTrajectory::ACTION_ABORT){
        has_traj_ = false;
        return;
    }

    traj_.header.frame_id = msg.header.frame_id;
    traj_.header.stamp    = msg.header.stamp;

    traj_.trajectory_id = msg.trajectory_id;
    traj_.num_segment   = msg.num_segment;

    traj_.time.clear();
    traj_.coef_x.clear();
    traj_.coef_y.clear();
    traj_.coef_z.clear();

    traj_.time      = msg.time;
    traj_.order     = msg.order;
    traj_.action    = msg.action;
    traj_.coef_x    = msg.coef_x;
    traj_.coef_y    = msg.coef_y;
    traj_.coef_z    = msg.coef_z;
    traj_.start_yaw = msg.start_yaw;
    traj_.final_yaw = msg.final_yaw;

    int order = traj_.order.front();
    C_.resize(order+1);
    Cv_.resize(order);
    Ca_.resize(order-1);
    Cj_.resize(order-2);
    for (int k = 0; k < order+1; ++k) {
        if (k < order+1)
            C_[k] = msg.C[k];
        if (k < order)
            Cv_[k] = msg.Cv[k];
        if (k < order-1)
            Ca_[k] = msg.Ca[k];
        if (k < order-2)
            Cj_[k] = msg.Cj[k];
    }

    t_traj_start_ = traj_.header.stamp.toSec();

    int n_seg = traj_.num_segment, n_poly = traj_.order[0] + 1;
    polyCoeff.resize(n_seg, 3 * n_poly);
    t_traj_sum_ = 0;
    for(int i = 0; i < n_seg; i++ )
    {
        t_traj_sum_ += traj_.time[i];
        for(int j = 0; j < 3 * n_poly; j++) {
            double var;
            if (j < n_poly)
                var = traj_.coef_x[n_poly * i + j];
            else if (j < 2 * n_poly)
                var = traj_.coef_y[n_poly * i + j - n_poly];
            else
                var = traj_.coef_z[n_poly * i + j - 2 * n_poly];

            polyCoeff(i , j) = var;
        }
    }

    has_traj_ = true;
}

Eigen::VectorXd getStateFromBezier(double t_now, int seg_now )
{
    Eigen::VectorXd ret = Eigen::VectorXd::Zero(12);

    Eigen::VectorXd ctrl_now = polyCoeff.row(seg_now);
    int ctrl_num1D = polyCoeff.cols() / 3;

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < ctrl_num1D; j++){
            ret[i] += C_(j) * ctrl_now(i * ctrl_num1D + j) * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j) );

            if(j < ctrl_num1D - 1 )
                ret[i+3] += Cv_(j) * traj_order_
                            * ( ctrl_now(i * ctrl_num1D + j + 1) - ctrl_now(i * ctrl_num1D + j))
                            * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j - 1) );

            if(j < ctrl_num1D - 2 )
                ret[i+6] += Ca_(j) * traj_order_ * (traj_order_ - 1)
                            * ( ctrl_now(i * ctrl_num1D + j + 2) - 2 * ctrl_now(i * ctrl_num1D + j + 1) + ctrl_now(i * ctrl_num1D + j))
                            * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j - 2) );

            if(j < ctrl_num1D - 3 )
                ret[i+9] += Cj_(j) * traj_order_ * (traj_order_ - 1) * (traj_order_ - 2)
                            * ( ctrl_now(i * ctrl_num1D + j + 3) - 3 * ctrl_now(i * ctrl_num1D + j + 2) + 3 * ctrl_now(i * ctrl_num1D + j + 1) - ctrl_now(i * ctrl_num1D + j))
                            * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j - 3) );
        }
    }

    return ret;
}

Eigen::VectorXd getTrajPoint(double time){
    int seg_now = 0, n_seg = traj_.num_segment;
    double t_bezier = 0, t_sum = 0;
    for (int i = 0; i < n_seg; ++i) {
        if (time >= t_sum && time <= (t_sum + traj_.time[i])){
            seg_now  = i;
            t_bezier = time - t_sum;
            break;
        } else {
            t_sum += traj_.time[i];
        }
    }

    Eigen::VectorXd ret;
    if (time <= t_traj_sum_)
        ret = getStateFromBezier(t_bezier/traj_.time[seg_now], seg_now);
    else
        ret = getStateFromBezier(1.0, n_seg-1);

    for (int j = 0; j < 12; ++j) {
        if (j < 3)
            ret(j) *= traj_.time[seg_now];
        else if(j >= 6 && j < 9)
            ret(j) /= traj_.time[seg_now];
        else if(j >= 9 && j < 12)
            ret(j) /= pow(traj_.time[seg_now], 2);
    }

    return ret;
}

void exitmain(){
    delete attom_;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "wmr_nav");
    ros::NodeHandle nh("~");

    ros::Subscriber pts_sub  = nh.subscribe("/waypoints", 10, rcvWaypointCallback);
    ros::Subscriber pos_sub  = nh.subscribe("/pos", 1, rcvPositionCallback);
    ros::Subscriber map_sub  = nh.subscribe("/ESDF_map", 1, rcvMapCallbak);
    ros::Subscriber traj_sub = nh.subscribe("/trajectory", 10, rcvTrajCallback);
    ros::Subscriber auto_sub = nh.subscribe("/is_auto_mode", 2,  rcvIsAutoMode);

    traj_Stime_pub_ = nh.advertise<std_msgs::Header>("/traj_start_time", 10);
    restart_pub_    = nh.advertise<std_msgs::Bool>("/restart_traj",   1);
    cmd_pub_        = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    double Vmax, Wmax, DVmax, DWmax;
    double _q1, _q2, _q3;
    double EXE_time;  // execute time
    double PRE_time;  // prediction period
    double robot_radius, dis_retraj;

    nh.param("navigation/max_v",        Vmax,  1.0);
    nh.param("navigation/max_w",        Wmax,  90*D2R);
    nh.param("navigation/max_dv",       DVmax,  0.5);
    nh.param("navigation/max_dw",       DWmax,  180*D2R);
    nh.param("navigation/execute_period",   EXE_time,  0.1);
    nh.param("navigation/prediction_period",PRE_time,  2.0);
    nh.param("navigation/is_angle_PID",     is_angle_PID_, false);

    nh.param("navigation/q1",    _q1,     1.0);
    nh.param("navigation/q2",    _q2,     1.0);
    nh.param("navigation/q3",    _q3,     0.5);

    nh.param("optimization/poly_order", traj_order_,     10.0);
    nh.param("optimization/min_order",  minimize_order_, 3.0);

    nh.param("robot/radius",  robot_radius, 0.2);
    nh.param("distance_retraj",  dis_retraj, 1.0);

    std::string file_path;
    nh.param<std::string>("record/file_path", file_path,
            std::string("/home/jg/Desktop/document/wmr_nav/"));

    std::string time_name, file_name;
    std::ostringstream ostr;
    //获取当前系统的当前日期/时间
    std::time_t now = std::time(0);
    std::tm *ltm = std::localtime(&now);
    /**********file name about time************/
    ostr << 1 + ltm->tm_mon << "_" << ltm->tm_mday << "_" << ltm->tm_hour << "_"
         << ltm->tm_min << "_" << ltm->tm_sec << ".nav";

    time_name = ostr.str();
    file_name = file_path + time_name;

    ROS_INFO("file name : %s", (file_name).c_str());

    out_file_.open(file_name.c_str());
    if (!out_file_) {
        std::cerr << "open output file error !" << std::endl;
        exit(-1);
    }

    attom_ = new ATTOM(Vmax, Wmax, DVmax, DWmax, EXE_time, PRE_time, robot_radius, dis_retraj);
    attom_->setQ(_q1, _q2, _q3);
    ros::Rate rate(50);
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    atexit(exitmain);
    return 0;
}

void ATTOM::setRobotState(Eigen::Vector3d& p_3d, double& yaw, double& v, double& w) {
    p_->x = p_3d(0);
    p_->y = p_3d(1);
    p_->yaw = yaw;
    p_->velocity = v;
    p_->angular  = w;

    if(out_file_) {
        file_mutex_.lock();
        out_file_ << "$p,"
                  << double2string(t_now_.toSec()) << ","
                  << double2string(p_->x) << "," << double2string(p_->y) << ","
                  << double2string(p_->yaw)      << ","
                  << double2string(p_->velocity) << ","
                  << double2string(p_->angular)  << std::endl;
        file_mutex_.unlock();
    }
}

void ATTOM::calcDynamicWindow(double& vmin, double& vmax, double& wmin, double& wmax) {
    double vd_vmin = p_->velocity - DVmax_ * exe_time_;
    double vd_vmax = p_->velocity + DVmax_ * exe_time_;
    double vd_wmin = p_->angular  - DWmax_ * exe_time_;
    double vd_wmax = p_->angular  + DWmax_ * exe_time_;

    vmin = vd_vmin >= Vmax_  ? Vmax_  : (vd_vmin < 0      ? 0      : vd_vmin);
    vmax = vd_vmax <= 0      ? 0      : (vd_vmax > Vmax_  ? Vmax_  : vd_vmax);
    wmin = vd_wmin >= Wmax_  ? Wmax_  : (vd_wmin < -Wmax_ ? -Wmax_ : vd_wmin);
    wmax = vd_wmax <= -Wmax_ ? -Wmax_ : (vd_wmax > Wmax_  ? Wmax_  : vd_wmax);
}

//确认控制算法是否准备好
//return ture  --准备进行控制
//return false --不能进行控制
bool ATTOM::isReady(bool is_PID_open) {
    if(is_PID_open && angleCtrl_) {
        this->anglePIDCtrl();
        return false;
    }
    else {
        if(!reachTarget_ && this->isFinish()) {
            reachTarget_ = true;
            if(is_PID_open)
                angleCtrl_ = true;
            return false;
        }
    }

    return true;
}

//控制间隔频率 0.1s
bool ATTOM::run(double& velocity, double& angular, bool is_PID_open) {
    if (!this->isReady(is_PID_open)) return true;

    if (isPeriodStart_) {
        if (!getPr()) {
            if(is_PID_open)
                angleCtrl_ = true;
            velocity   = 0;
            angular    = 0;
            return false;
        }
        start_time_ = t_now_.toSec();
        isPeriodStart_ = false;

//        if (this->isSwitch()){
//            e2_ = 0;
//            e3_ = 3;
//            e4_ = 2;
//        } else {
//            e2_ = 1;
//            e3_ = 0;
//            e4_ = 0;
//        }

        //计算出可行速度窗口
        calcDynamicWindow(vmin_, vmax_, wmin_, wmax_);

        //计算窗口内的可行速度的预估轨迹和三项系数
        evaluation();
        //无可行路径
        if (vec_eval_.empty()) {
            ROS_WARN("no path to goal !");
            velocity    = 0;
            angular     = 0;
            start_time_ = 0;
            isPeriodStart_ = true;
            isReTraj_      = true;
            if(is_PID_open)
                angleCtrl_ = true;
            return false;
        }
        //系数正规化
        normalize();
        //最终评价
        finalEvaluate();
    }

    if (t_now_.toSec() - start_time_ >= exe_time_) {
        isPeriodStart_ = true;
        this->run(velocity, angular);
    }

    velocity = v_output_;
    angular  = w_output_;

    return true;
}

bool ATTOM::getPr() {
    std::vector<RobState>().swap(vec_pr_);
    double x, y, yaw, v, w;
    double t_traj = t_now_.toSec() - t_traj_start_;
    file_mutex_.lock();
    for(double t = t_traj; t <= t_traj + pre_time_; t += exe_time_){
        if(t <= t_traj_sum_){
            auto p = getTrajPoint(t);
            x = p(0); y = p(1);
            yaw = atan2(p(4), p(3));
            v = sqrt(pow(p(3), 2) + pow(p(4), 2));
            w = (p(7)*p(3) - p(6)*p(4))/(pow(p(3), 2) + pow(p(4), 2));
            vec_pr_.emplace_back(RobState(x, y, yaw, v, w));

            if(t == t_traj){
                out_file_ << "$pr_va,"
                          << double2string(t_now_.toSec()) << ","
                          << double2string(t_traj) << ","
                          << double2string(p(6)) << ","
                          << double2string(p(7)) << ","
                          << double2string(p(8)) << std::endl;
            }
        }
        else break;
    }

    if(!vec_pr_.empty()){
        auto pr_first = vec_pr_.front();
        out_file_ << "$pr,"
                  << double2string(t_now_.toSec()) << ","
                  << double2string(pr_first.x) << ","
                  << double2string(pr_first.y) << ","
                  << double2string(pr_first.yaw)      << ","
                  << double2string(pr_first.velocity) << ","
                  << double2string(pr_first.angular)  << std::endl;

        if(pow((p_->x - pr_first.x), 2) + pow((p_->y - pr_first.y), 2) >= pow(dis_retraj_, 2)){
            ROS_WARN("Too far to track reference trajectory --");
            isReTraj_ = true;
        }
    }
    else {
        ROS_WARN("no pr ----");
        isReTraj_ = true;
    }

    file_mutex_.unlock();
    return !isReTraj_;
}

void ATTOM::evaluation() {
    std::vector<RobState> traj_for_one;
    std::vector<EvalATTOM>().swap(vec_eval_);
    std::vector<QE>().swap(vec_QE_);
    qe_sum_ = 0;
    te_sum_ = 0;
    de_sum_ = 0;
    ve_sum_ = 0;
    file_mutex_.lock();
    for (double vt = vmin_; vt <= vmax_;) {
        for (double wt = wmin_; wt <= wmax_;) {
            double dis_obs = 0, qe_eval = 0;
            if(!calcQE(vt, wt, qe_eval, dis_obs, traj_for_one)){
//                out_file_ << "$check," << dis_obs << "," << vt << "," << wt << std::endl;
                wt += WRESL;
                continue;
            }
            dis_obs = 10/dis_obs;

            double term_cst = CalcTermC(traj_for_one.back());
            double vel_eval = CalcVel(vt);

            vec_eval_.emplace_back(EvalATTOM(vt, wt, qe_eval, term_cst, dis_obs, vel_eval));
            qe_sum_ += qe_eval;
            te_sum_ += term_cst;
            de_sum_ += dis_obs;
            ve_sum_ += vel_eval;

            wt += WRESL;

            auto &predict_point = traj_for_one.back();
            out_file_ << "$predict_traj,"
                      << double2string(t_now_.toSec()) << ","
                      << double2string(predict_point.x) << ","
                      << double2string(predict_point.y) << ","
                      << double2string(predict_point.yaw) << ","
                      << double2string(predict_point.velocity) << ","
                      << double2string(predict_point.angular) << std::endl;
        }
        vt += VRESL;
    }

    file_mutex_.unlock();
    std::vector<RobState>().swap(traj_for_one);
}

//生成预估轨迹,轨迹为以(x_circle_, y_circle_)为圆心的曲率圆的圆弧
void ATTOM::genTrajPoint(RobState& p_eval, const double& vt, const double& wt, const double& t) {
    //曲率圆的半径, wt为负时r为负, (x_circle_, y_circle_)为圆心
    double r_circle = vt / wt;
    if(std::isinf(r_circle)){
        p_eval.x += vt * t * cos(p_eval.yaw);
        p_eval.y += vt * t * sin(p_eval.yaw);
    } else {
        double x_circle = p_eval.x - r_circle * sin(p_eval.yaw);
        double y_circle = p_eval.y + r_circle * cos(p_eval.yaw);
        //将终点坐标赋予p_eval
        p_eval.x = x_circle + r_circle * cos(p_eval.yaw - M_PI/2 + wt * t);
        p_eval.y = y_circle + r_circle * sin(p_eval.yaw - M_PI/2 + wt * t);
        p_eval.yaw += wt * t;
    }

    p_eval.velocity = vt;
    p_eval.angular  = wt;
}

bool ATTOM::calcQE(const double& vt, const double& wt, double& qe, double& dis, std::vector<RobState>& traj) {
    std::vector<RobState>().swap(traj);

    auto ipr = vec_pr_.begin();
    int index_t = 0;
    double dis_min = INT_MAX, dis_tmp;
    double qe_ret = 0, xe, ye;
    for (; ipr != vec_pr_.end(); ++ipr){

        RobState p_ev(p_->x, p_->y, p_->yaw, p_->velocity, p_->angular);
        genTrajPoint(p_ev, vt, wt, index_t * exe_time_);

        dis_tmp = getDistanceCost(p_ev.x, p_ev.y);
        if (dis_tmp <= robot_radius_){
            std::vector<RobState>().swap(traj);
            return false;
        }
        if (dis_tmp < dis_min)
            dis_min = dis_tmp;

        //calculate the deviation from state vector with reference track point
        xe = cos(p_ev.yaw) * (ipr->x - p_ev.x) + sin(p_ev.yaw) * (ipr->y - p_ev.y);
        ye = -sin(p_ev.yaw) * (ipr->x - p_ev.x) + cos(p_ev.yaw) * (ipr->y - p_ev.y);
        double thetae = ipr->yaw - p_ev.yaw;
        while (thetae > M_PI) {
            thetae -= 2*M_PI;
        }
        while (thetae < -M_PI) {
            thetae += 2*M_PI;
        }
        //sum the deviation
        traj.emplace_back(p_ev);
        qe_ret += q1_ * pow(xe, 2) + q2_ * pow(ye, 2)  + q3_ * pow(thetae, 2);
        index_t++;
    }

    qe  = qe_ret;
    dis = dis_min;
    return true;
}

//calculate the constraint of terminal
double ATTOM::CalcTermC(RobState& pt) {
    auto point_pr = vec_pr_.back();
    double xe = cos(pt.yaw) * (point_pr.x - pt.x) + sin(pt.yaw) * (point_pr.y - pt.y);
    double ye = -sin(pt.yaw) * (point_pr.x - pt.x) + cos(pt.yaw) * (point_pr.y - pt.y);
    double thetae = point_pr.yaw - pt.yaw;
    while (thetae > M_PI) {
        thetae -= 2*M_PI;
    }
    while (thetae < -M_PI) {
        thetae += 2*M_PI;
    }

    vec_QE_.emplace_back(QE(xe, ye, thetae));

    return 0.5 * (pow(xe, 2) + pow(ye, 2) + pow(thetae, 2)) + 1 - cos(thetae);
}

double ATTOM::CalcVel(double _v) {
    double v_reference = vec_pr_.front().velocity;
    return _v <= v_reference ? -_v + v_reference : _v - v_reference;
//    return VMAX - _v;
}

void ATTOM::finalEvaluate() {
    double feval = 0, tempVST = 0;
//    int ki = 0;
    for (int i = 0; i < (int)vec_eval_.size(); ++i) {
        tempVST = vec_eval_[i].qe * e1_
                  + vec_eval_[i].te * e2_
                  + vec_eval_[i].de * e3_
                  + vec_eval_[i].ve * e4_;
        if (i == 0) feval = tempVST;

        if (tempVST <= feval) {
            feval = tempVST;
            v_output_ = vec_eval_.at(i).v;
            w_output_ = vec_eval_.at(i).w;
//            ki = i;
        }
    }

    file_mutex_.lock();
    out_file_ << "$cmd," << double2string(t_now_.toSec()) << ","
    << double2string(v_output_) << ","
    << double2string(w_output_) << std::endl;
    writeError();
    file_mutex_.unlock();
}

void ATTOM::writeError() {
    auto point_pr = vec_pr_.front();
    double xe = cos(p_->yaw) * (point_pr.x - p_->x) + sin(p_->yaw) * (point_pr.y - p_->y);
    double ye = -sin(p_->yaw) * (point_pr.x - p_->x) + cos(p_->yaw) * (point_pr.y - p_->y);
    double thetae = point_pr.yaw - p_->yaw;
    while (thetae > M_PI) {
        thetae -= 2*M_PI;
    }
    while (thetae < -M_PI) {
        thetae += 2*M_PI;
    }

    out_file_ << "$error," << double2string(t_now_.toSec()) << ","
              << double2string(xe) << ","
              << double2string(ye) << ","
              << double2string(thetae) << std::endl;
}

void ATTOM::normalize() {
    for (int i = 0; i < (int)vec_eval_.size(); ++i) {
        if (qe_sum_ != 0) {
            vec_eval_.at(i).qe /= qe_sum_;
        }
        if (te_sum_ != 0) {
            vec_eval_.at(i).te /= te_sum_;
        }
        if (de_sum_ != 0) {
            vec_eval_.at(i).de /= de_sum_;
        }
        if (ve_sum_ != 0) {
            vec_eval_[i].ve /= ve_sum_;
        }

//        double dp = vec_eval_.at(i)->qe
//                    + vec_eval_.at(i)->te
//                    + vec_eval_.at(i)->de
//                    + vec_eval_[i]->ve;
//        out_file_ << "$norEval," << vec_eval_.at(i)->qe << "," << vec_eval_.at(i)->te
//                  << "," << vec_eval_.at(i)->de
//                  << "," << vec_eval_[i]->ve
//                  << "," << vec_eval_.at(i)->v << "," << vec_eval_.at(i)->w
//                  << "," << dp <<  std::endl;
    }
}

/* vector pr in obstacle -true; else -false*/
bool ATTOM::isSwitch() {
    auto iter_pr = vec_pr_.begin();
    for ( ; iter_pr != vec_pr_.end(); ++iter_pr){
        auto pr = *iter_pr;
        if (getDistanceCost(pr.x, pr.y) <= 0.3)
            return true;
    }
    return false;
}
/*** 角度PID控制 --机器人原地自转到预期角度  ***/
void ATTOM::anglePIDCtrl() {
    reachTarget_ = false;

    auto p_begin = getTrajPoint(0);
    auto p_next  = getTrajPoint(traj_.time.front());
    double yaw_begin = atan2(p_next(1)-p_begin(1), p_next(0)-p_begin(0));
//    double yaw_begin = traj_.start_yaw;
    std::unique_ptr<PidController> anglePID(new PidController(0.5, 0.5, 0, 0, 0.57, -0.57));
//    auto anglePID = new PidController(0.5, 0.5, 0, 0, 1, -1);

    if (yaw_begin - p_->yaw  > M_PI)
        yaw_begin -= 2 * M_PI;
    else if (yaw_begin - p_->yaw  < -M_PI)
        yaw_begin += 2 * M_PI;

    if (yaw_begin - p_->yaw >= (double)M_PI / 24 || yaw_begin - p_->yaw <= -(double)M_PI / 24) {

        anglePID->pidSetTarget(yaw_begin);
        speed_command_.linear.x  = 0;
        speed_command_.angular.z = anglePID->pidOperation(p_->yaw);
        cmd_pub_.publish(speed_command_);
    }
    else {
        speed_command_.linear.x  = 0;
        speed_command_.angular.z = 0;
        cmd_pub_.publish(speed_command_);
        ros::Duration(1).sleep();

        t_traj_start_ = t_now_.toSec();
        angleCtrl_    = false;

        std_msgs::Header traj_start_time;
        traj_start_time.stamp = t_now_;
        traj_Stime_pub_.publish(traj_start_time);
        ROS_WARN("angle ctrl ---------------------");
    }
}

//true - the distance between position of robot and target point is less than 0.1 m
bool ATTOM::isFinish() {
    const auto& end_p = getTrajPoint(t_traj_sum_);

    double dis = pow((p_->x - end_p(0)), 2) + pow((p_->y - end_p(1)), 2);
    if(dis <= pow(0.1, 2) || (!angleCtrl_ && t_now_.toSec() >= t_traj_start_ + t_traj_sum_)){
        reachTarget_ = true;
    }
    else reachTarget_ = false;

    return reachTarget_;
}
